package com.huayun.plugin.fastener.utils;

import org.apache.commons.math3.linear.*;

import java.util.ArrayList;
import java.util.List;

public class CoordinateTransform {
    public static List<Double> calculateTransformMatrix(double[] partStart, double[] partEnd,
                                                        double[] assemblyStart, double[] assemblyEnd) {
        // 1. 计算质心
        double[] centroidPart = calculateCentroid(partStart, partEnd);
        double[] centroidAssembly = calculateCentroid(assemblyStart, assemblyEnd);

        // 2. 去中心化坐标
        double[] partStartCentered = subtract(partStart, centroidPart);
        double[] partEndCentered = subtract(partEnd, centroidPart);
        double[] assemblyStartCentered = subtract(assemblyStart, centroidAssembly);
        double[] assemblyEndCentered = subtract(assemblyEnd, centroidAssembly);

        // 3. 构造协方差矩阵 H = Σ (P_part' * P_assembly'^T)
        RealMatrix H = MatrixUtils.createRealMatrix(3, 3);
        H = H.add(outerProduct(partStartCentered, assemblyStartCentered))
                .add(outerProduct(partEndCentered, assemblyEndCentered));

        // 4. SVD分解
        SingularValueDecomposition svd = new SingularValueDecomposition(H);
        RealMatrix U = svd.getU();
        RealMatrix V = svd.getVT().transpose(); // VT是V的转置，所以V = VT^T

        // 5. 计算旋转矩阵 R = V * U^T
        RealMatrix R = V.multiply(U.transpose());

        // 确保行列式为正值（避免镜像）
        if (new LUDecomposition(R).getDeterminant() < 0) {
            V.setColumn(2, V.getColumnVector(2).mapMultiply(-1).toArray()); // 调整V的第三列符号
            R = V.multiply(U.transpose());
        }

        // 6. 计算平移向量 t = C_assembly - R * C_part
        RealVector t = new ArrayRealVector(centroidAssembly)
                .subtract(R.operate(new ArrayRealVector(centroidPart)));

        // 7. 组合4x4变换矩阵
        RealMatrix T = MatrixUtils.createRealMatrix(4, 4);
        T.setSubMatrix(R.getData(), 0, 0);
        T.setColumnVector(3, t.append(1.0)); // 平移向量放在第四列

        return turnMatrix(T);
    }
    public static List<Double> calculateTransformMatrix2(double[] partStart, double[] partEnd,
                                                         double[] assemblyStart, double[] assemblyEnd) {
        double[] origin = {0,0,0};
        //平移
        double[][] translateMatrix1 = CreateTranslationMatrix(partStart,origin);
        //旋转
        double[][] rotateMatrix = CreateRotationMatrix(partStart,partEnd,assemblyStart,assemblyEnd);
        //平移
        double[][] translateMatrix2 = CreateTranslationMatrix(origin,assemblyStart);
        //缩放
        //double[][] scaleMatrix = CreateScaleMatrix(partStart,partEnd,assemblyStart,assemblyEnd);

        //相乘
        //double[][] resultMatrix = MultiplyMatrix(MultiplyMatrix(translateMatrix,rotateMatrix),scaleMatrix);   //有缩放
        double[][] resultMatrix = MultiplyMatrix(MultiplyMatrix(translateMatrix2,rotateMatrix),translateMatrix1);     //无缩放

        List<Double> matrix = new ArrayList<>();
        for (int i = 0; i < 4; i++) {
            for (int j = 0; j < 4; j++) {
                matrix.add(Double.parseDouble(String.format("%.4f", resultMatrix[i][j])));
            }
        }
        return matrix;
    }

    //计算平移矩阵
    private static double[][] CreateTranslationMatrix(double[] point1,double[] point2)
    {
        return new double[][]{
                {1,0,0,point2[0] - point1[0]},
                {0,1,0,point2[1] - point1[1]},
                {0,0,1,point2[2] - point1[2]},
                {0,0,0,1}
        };
    }
    //计算旋转矩阵
    private  static double[][] CreateRotationMatrix(double[] point11,double[] point12,double[] point21,double[] point22)
    {
        double[] v1 = {point12[0] - point11[0],point12[1] - point11[1],point12[2] - point11[2]};
        double[] v2 = {point22[0] - point21[0],point22[1] - point21[1],point22[2] - point21[2]};

        //归一化处理
        double length1 = Math.sqrt(v1[0] * v1[0] + v1[1] * v1[1] + v1[2] * v1[2]);
        double length2 = Math.sqrt(v2[0] * v2[0] + v2[1] * v2[1] + v2[2] * v2[2]);
        double[] v1Normal = {v1[0] / length1,v1[1] / length1,v1[2] / length1};
        double[] v2Normal = {v2[0] / length2,v2[1] / length2,v2[2] / length2};

        //旋转轴 叉乘n = v1 X v2
        //旋转角度 theta = arccos(v1 . v2)
        double[] axis = {
                v1Normal[1] * v2Normal[2] -  v1Normal[2] * v2Normal[1],
                v1Normal[2] * v2Normal[0] -  v1Normal[0] * v2Normal[2],
                v1Normal[0] * v2Normal[1] -  v1Normal[1] * v2Normal[0]
        };
        double angle = Math.acos(v1Normal[0] * v2Normal[0] + v1Normal[1] * v2Normal[1] + v1Normal[2] * v2Normal[2]);

        //罗德里格斯旋转公式
        double cos = Math.cos(angle);
        double sin = Math.sin(angle);

        double[][] R = {
                {cos + axis[0] * axis[0] * (1 - cos),axis[0] * axis[1] * (1 - cos) - axis[2] * sin,axis[0] * axis[2] * (1 - cos) + axis[1] * sin,0},
                {axis[1] * axis[0] * (1 - cos) + axis[2] * sin,cos + axis[1] * axis[1] * (1 - cos),axis[1] * axis[2] * (1 - cos) - axis[0] * sin,0},
                {axis[2] * axis[0] * (1 - cos) - axis[1] * sin,axis[2] * axis[1] * (1 - cos) + axis[0] * sin,cos + axis[2] * axis[2] * (1 - cos),0},
                {0,0,0,1}
        };

        return R;
    }

    //缩放矩阵
    private static double[][] CreateScaleMatrix(double[] point11,double[] point12,double[] point21,double[] point22)
    {
        double[] v1 = {point12[0] - point11[0],point12[1] - point11[1],point12[2] - point11[2]};
        double[] v2 = {point22[0] - point21[0],point22[1] - point21[1],point22[2] - point21[2]};

        double length1 = Math.sqrt(v1[0] * v1[0] + v1[1] * v1[1] + v1[2] * v1[2]);
        double length2 = Math.sqrt(v2[0] * v2[0] + v2[1] * v2[1] + v2[2] * v2[2]);

        double s = length2 / length1;
        double[][] scaleMatrix = {
                {s,0,0,0},
                {0,s,0,0},
                {0,0,s,0},
                {0,0,0,1}
        };
        return scaleMatrix;
    }

    //矩阵相乘
    private static double[][] MultiplyMatrix(double[][] matrix1,double[][] matrix2)
    {
        double[][] result = new double[4][4];
        for(int i = 0;i < 4;i++)
        {
            for(int j = 0;j < 4;j++)
            {
                for(int k = 0;k < 4;k++)
                {
                    result[i][j] += matrix1[i][k] * matrix2[k][j];
                }
            }
        }
        return result;
    }


    // 辅助方法：计算质心
    private static double[] calculateCentroid(double[] a, double[] b) {
        return new double[]{
                (a[0] + b[0]) / 2,
                (a[1] + b[1]) / 2,
                (a[2] + b[2]) / 2
        };
    }

    // 辅助方法：向量相减
    private static double[] subtract(double[] a, double[] b) {
        return new double[]{a[0] - b[0], a[1] - b[1], a[2] - b[2]};
    }

    // 辅助方法：外积计算
    private static RealMatrix outerProduct(double[] a, double[] b) {
        RealMatrix matrix = MatrixUtils.createRealMatrix(3, 3);
        for (int i = 0; i < 3; i++) {
            for (int j = 0; j < 3; j++) {
                matrix.setEntry(i, j, a[i] * b[j]);
            }
        }
        return matrix;
    }

    // 辅助方法：打印矩阵
    private static List<Double> turnMatrix(RealMatrix matrix) {
        List<Double> matrixs = new ArrayList<>();
        for (int i = 0; i < matrix.getRowDimension(); i++) {
            for (int j = 0; j < matrix.getColumnDimension(); j++) {
                matrixs.add(Double.parseDouble(String.format("%.4f", matrix.getEntry(i, j))));
            }
        }
        return matrixs;
    }

    public static List<Double> calculateTransformMatrixThreePoints(
            double[] pointA, double[] pointB, double[] pointC,
            double[] pointA1, double[] pointB1, double[] pointC1) {

        // 1. 计算质心
        double[] centroid1 = calculateCentroidThreePoints(pointA, pointB, pointC);
        double[] centroid2 = calculateCentroidThreePoints(pointA1, pointB1, pointC1);

        // 2. 去中心化坐标
        double[] A_centered = subtract(pointA, centroid1);
        double[] B_centered = subtract(pointB, centroid1);
        double[] C_centered = subtract(pointC, centroid1);

        double[] A1_centered = subtract(pointA1, centroid2);
        double[] B1_centered = subtract(pointB1, centroid2);
        double[] C1_centered = subtract(pointC1, centroid2);

        // 3. 构造协方差矩阵 H
        RealMatrix H = MatrixUtils.createRealMatrix(3, 3);
        H = H.add(outerProduct(A_centered, A1_centered))
                .add(outerProduct(B_centered, B1_centered))
                .add(outerProduct(C_centered, C1_centered));

        // 4. SVD分解
        SingularValueDecomposition svd = new SingularValueDecomposition(H);
        RealMatrix U = svd.getU();
        RealMatrix V = svd.getVT().transpose();

        // 5. 计算旋转矩阵 R
        RealMatrix R = V.multiply(U.transpose());

        // 确保行列式为正值
        if (new LUDecomposition(R).getDeterminant() < 0) {
            V.setColumn(2, V.getColumnVector(2).mapMultiply(-1).toArray());
            R = V.multiply(U.transpose());
        }

        // 6. 计算平移向量 t
        RealVector t = new ArrayRealVector(centroid2)
                .subtract(R.operate(new ArrayRealVector(centroid1)));

        // 7. 组合4x4变换矩阵
        RealMatrix T = MatrixUtils.createRealMatrix(4, 4);
        T.setSubMatrix(R.getData(), 0, 0);
        T.setColumnVector(3, t.append(1.0));

        return turnMatrix(T);
    }

    private static double[] calculateCentroidThreePoints(double[] a, double[] b, double[] c) {
        return new double[]{
                (a[0] + b[0] + c[0]) / 3,
                (a[1] + b[1] + c[1]) / 3,
                (a[2] + b[2] + c[2]) / 3
        };
    }
}
